#ifndef VEHICLE_HPP
#define VEHICLE_HPP

#include "veye.hpp"

#include "platemap.hpp"

#include "Dstar.h"
#include "tost.hpp"

#include "iscanner.hpp"

//#include "server.hpp"
//#include "vehicle.hpp"
#include "follower.hpp"

#include "commandserver.hpp"

#include <map>

class Vehicle : public CommandServer, public VEye
{
public:

    //    DetectServer detectServer;

    std::map<std::string, cv::Vec3d> toTargetMap;

//    AGV坐标系xOy定义：
//    以定位相机的镜头中心为原点O， y轴为镜头光轴在水平面的投影，方向从镜头内部指向镜头外部，x轴沿水平面并与y轴垂直，方向与相机图像x轴方向一致

//    AGV位置和姿态的定义cv::Vec3d即为AGV坐标系xOy到地面坐标系xOy的转换


    Vehicle()
        : CommandServer()
        , VEye()
    {
        BuildTargetMap2();
    }


    void BuildTargetMap()
    {
        toTargetMap["10"]=cv::Vec3d(0, 900, CV_PI);
        toTargetMap["89"]=cv::Vec3d(2150, 900, CV_PI);
        toTargetMap["67"]=cv::Vec3d(2150, 7500, 0);
    }



    void BuildTargetMap2()
    {
        toTargetMap["10"]=cv::Vec3d(0, 900, CV_PI);
        toTargetMap["89"]=cv::Vec3d(1810, 900, CV_PI);
        toTargetMap["67"]=cv::Vec3d(1810, 7500, 0);
    }



    void BuildTargetMap1()
    {
        toTargetMap["10"]=cv::Vec3d(2150, 2500, 0);
        toTargetMap["89"]=cv::Vec3d(2150, 500, CV_PI);
    }


    void Analyze(DoubleBf2<AnalysisResultA> &result)
    {
        PlateMap pmap;

        cv::Mat D;
        double angle=0;

        AnalysisResultA *par=NULL;

        while(true)
        {
            cv::Mat *pm=NULL;

            //std::cout<<"read pm\n"<<std::flush;
            frameBuffer.Read(pm);

            if(pm!=NULL)
            {
                //std::cout<<"analysis pm\n"<<std::flush;

                par=new AnalysisResultA();

                pmap.UpdateLocation(*pm, Knew, D, angle, *par);

                par->frame=pm;

                result.Write(par);

                //        delete pm;
            }
            else
            {
                //std::cout<<"null pm\n"<<std::flush;
                break;
            }
        }

        par=NULL;
        result.Write(par);
        std::cout<<"end Analyze\n"<<std::flush;
    }


    static void AnalyzeThread(Vehicle *pData,DoubleBf2<AnalysisResultA> &result)
    {
        pData->Analyze(result);
    }


    static cv::Vec3f ComputeSpeedA(cv::Vec2d goal, Dstar &planner, double dfRad)
    {
        std::list<state> mypath;

        planner.updateGoal(goal[0],goal[1]);
        planner.replan();               // plan a path
        mypath = planner.getPath();     // retrieve path

        std::vector<cv::Point> mpath;

        for (std::list<state>::iterator it = mypath.begin(); it != mypath.end(); it++)
        {
            mpath.push_back(cv::Point(it->x, it->y));
        }

        std::vector<cv::Point> approxPath;

        cv::approxPolyDP(mpath, approxPath, 5, false);

        //            std::cout<<mpath.size()<<'\t'<<approxPath.size()<<'\n';

        cv::Vec3f direction(approxPath[1].x-approxPath[0].x, approxPath[1].y-approxPath[0].y, 0);

        double distance=cv::norm(direction);

        //        double v=Follower::interp(distance, 50, 1500, 0, 100);
        double v=Follower::interp(distance, 50, 1500, 40, 250);


        cv::Vec3f speed=direction*v/cv::norm(direction);//50

        if(cv::norm(goal)<13000)
        {
            speed[2]=Follower::interp(dfRad, -0.4, 0.4, 0.1, -0.1);
        }

        if(dfRad>CV_PI/4)
        {
            speed[2]=-0.3;
        }
        if(dfRad<-CV_PI/4)
        {
            speed[2]=0.3;
        }

        return speed;

    }

    static void FixRadDiff(double &rad)
    {
        while(rad>CV_PI)rad-=CV_2PI;
        while(rad<=-CV_PI)rad+=CV_2PI;
    }

    static void Control(DoubleBf2<AnalysisResultA> &in, DoubleBf2<AnalysisResultA> &out, cv::Vec2d goal=cv::Vec2d(0,650), double targetRad=0)
    {
        float distancethresh = 30;
        double propertyThres=0.3;

        float radThres=CV_PI/90;

        ToST tst;

        tst.Initial();

        Dstar plan;

        plan.init(0,0,goal[0],goal[1]);         // set start to (0,0) and goal to (10,5)

        AnalysisResultA *par=NULL;

        cv::Vec3d speedOut(0,0,0);

        cv::Vec2d goalAtCar(0,1);

        size_t lostCount=0;

        double dfRad=0;

        while(true)
        {
            in.Read(par);

            if(par!=NULL)
            {
                cv::Vec3d speed(0,0,0);
                if(par->property<=propertyThres)
                {
                    std::cout<<"no target stop !"<<"\n";
                    std::cout<<"last goal at "<<goalAtCar<<"\n"<<std::flush;

                    lostCount++;


                    if(lostCount>9)
                    {

                        if(goalAtCar[0]<0)
                            speed=cv::Vec3d(0,0,0.1);
                        else
                            speed=cv::Vec3d(0,0,-0.1);
                    }
                    else
                    {
                        cv::Vec3f v=ComputeSpeedA(goalAtCar, plan, dfRad);

                        std::cout<<"v="<<v<<"\n"<<std::flush;

                        speed=cv::Vec3d(v[0],v[1],v[2]);
                    }

                }
                else
                {
                    lostCount=0;

                    goalAtCar=par->ToCarCoordinate(goal);

                    dfRad=par->location[2]-targetRad;
                    FixRadDiff(dfRad);

                    if(abs(goalAtCar[0])<distancethresh && abs(goalAtCar[1])<distancethresh && abs(dfRad)<radThres)
                    {
                        std::cout<<"reached"<<"\n"<<std::flush;
                        break;
                    }
                    else
                    {
                        cv::Vec3f v=ComputeSpeedA(goalAtCar, plan, dfRad);

                        std::cout<<"v="<<v<<"\n"<<std::flush;

                        speed=cv::Vec3d(v[0],v[1],v[2]);

                    }
                }

                Follower::LowPass(speedOut, speed);

                std::cout<<"speedOut="<<speedOut<<"\n"<<std::flush;

                tst.Move(speedOut[0],speedOut[1],speedOut[2]);

                out.Write(par);
            }
            else
            {
                std::cout<<"null pm\n"<<std::flush;
                break;
            }
            std::cout.flush();
        }

        par=NULL;
        out.Write(par);

        tst.Move();

        tst.End();

        std::cout<<"end Control\n"<<std::flush;
    }

    virtual void DisplayC(std::string command, cv::Vec2d goal, double goalRad)
    {
        cv::namedWindow(command,cv::WINDOW_NORMAL);
        cv::moveWindow(command,32,32);
        cv::resizeWindow(command, 1280, 720);

        DoubleBf2<AnalysisResultA> bf2;
        DoubleBf2<AnalysisResultA> bf3;

        std::thread th2(AnalyzeThread, this, std::ref(bf2));
        std::thread th3(Control, std::ref(bf2), std::ref(bf3), goal, goalRad);

        while(true)
        {
            AnalysisResultA *par=NULL;
            bf3.Read(par);
            if(par!=NULL)
            {
                par->Draw();
                cv::imshow(command, *(par->frame));
                k=cv::waitKey(10);
                delete par;
            }
            else
            {
                break;
            }
        }

        play=false;

        std::this_thread::sleep_for(std::chrono::milliseconds(500));

        cv::Mat* pm=NULL;

        frameBuffer.Write(pm);

        if(th2.joinable()) th2.join();

        if(th3.joinable()) th3.join();

        cv::destroyWindow(command);

        std::cout<<"end DisplayC\n"<<std::flush;

    }

    void R90(bool left=true)
    {
        ToST tst;
        tst.Initial();
        tst.R90(left);
        tst.End();
    }


    void R180(bool left=true)
    {
        ToST tst;
        tst.Initial();
        tst.R180(left);
        tst.End();
    }


    void Y(double distmm)
    {
        ToST tst;
        tst.Initial();
        tst.Y(distmm);
        tst.End();
    }





    bool GoToTarget(std::string commandTarget)
    {
        std::map<std::string, cv::Vec3d>::iterator it=toTargetMap.find(commandTarget);
        if(it==toTargetMap.end())
            return false;

        cv::Vec3d targetLocation=it->second;
        //        std::cout<<targetLocation<<"\n";
        DisplayC(commandTarget, cv::Vec2d(targetLocation[0], targetLocation[1]), targetLocation[2]);
        return true;
    }


    void test()
    {
        if(Wake(0))
        {

            R180();

            std::this_thread::sleep_for(std::chrono::milliseconds(500));

            Y(2000);
//            GoToTarget("10");
                        GoToTarget("89");
//                        GoToTarget("67");


            //            R90();
            //            DisplayC("TO10", cv::Vec2d(2150, 2500), 0);
            //            R90();
            play=true;
            //            R90();
            //            DisplayC("TO89", cv::Vec2d(2150, 500), CV_PI);
            //            R90();
            play=true;

            Display("IDLE");
        }
        VEye::End();
    }

    void start(uint16_t portno=7654)
    {

        if(Wake(0))
        {
            std::cout<<"starting"<<endl;

            CommandServer::Start(portno);


            while(k!=27)
            {
                VehicleCommand *pcmd=NULL;
                command.Read(pcmd);
                if(pcmd!=NULL)
                {
                    std::string verb=pcmd->command.substr(0,2);

                    if(verb.compare("TO")==0)
                    {
                        statusLock.lock();
                        status.status=verb;
                        std::string location0=status.location;
                        status.location=pcmd->command.substr(2,2);
                        statusLock.unlock();

                        std::cout<<"execute "<<pcmd->command<<"\n"<<std::flush;

                        play=true;

                        if(GoToTarget(pcmd->command.substr(2,2)))
                        {
                            log.AddLog(pcmd->command);
                            log.Save("log.txt");

                            statusLock.lock();
                            status.status="AT";
                            status.location=pcmd->command.substr(2,2);
                            statusLock.unlock();
                        }
                        else
                        {
                            statusLock.lock();
                            status.status="AT";
                            status.location=location0;
                            statusLock.unlock();
                        }

                        std::cout<<"end execute "<<pcmd->command<<"\n"<<std::flush;
                    }

                    if(verb.compare("OP")==0)
                    {
                        int lockerIndex=-1;
                        std::istringstream(pcmd->command.substr(2,2))>>lockerIndex;

                        statusLock.lock();
                        status.status="LD";
                        statusLock.unlock();

                        std::cout<<"execute "<<pcmd->command<<"\n"<<std::flush;


                        ToST tst;
                        tst.Initial();
                        tst.Unlock(lockerIndex);
                        tst.End();



                        statusLock.lock();
                        status.status="AT";

                        if(status.lockerID[lockerIndex].empty())
                            status.lockerID[lockerIndex]=pcmd->clientID;
                        else
                            status.lockerID[lockerIndex]="";
                        statusLock.unlock();
                        std::cout<<"end execute "<<pcmd->command<<"\n"<<std::flush;

                    }

                    delete pcmd;

                }

            }


            CommandServer::End();


        }
        VEye::End();
    }


};




#endif // VEHICLE_HPP

